package com.rockwell.sniffyhunter.model;

import java.util.Map;

import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.ObstacleSkipperState;

/**
 * This class provides the robot to navigate away from obstacles
 * The following actions will be performed:
 * <ul>
 * <li>Move away.
 * <li>Move forward.
 * <li>Move back.
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class ObstacleSkipper extends StateObject {
	
	private static final int MOVE_INDEX = 50; //mm
	private int ROTATE_DIRECTION = 1; //1:right, -1:left
	private int ROTATE_ANGLE = 90;
	private RobotCommandSequencer m_commandSequencer;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;
	private Map<Integer, IDetector> m_colorDetectors;
	private int m_deviationCount;

	public ObstacleSkipper(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_positionDetector = robot.m_positionDetector;
		m_colorDetectors = robot.m_colorDetectors;
	}

	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = ObstacleSkipperState.START;
		machineRecoveryState = machineState;
	}
	
	@Override
	public boolean IsDone() {
		return machineState == ObstacleSkipperState.DONE;
	}
	
	public IStateMachine start() {

		m_deviationCount = 0;
		
		machineState = ObstacleSkipperState.SCAN_BEFORE_ROTATE_TO_MOVE_AWAY;
		return machineState;
	}
	
	public IStateMachine scanBeforeRotateToMoveAway() {

		m_robotPosition = m_positionDetector.getPosition();
		
		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.ObstacleSkipper", 4);
			ROTATE_ANGLE = ROTATE_DIRECTION * 90;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_AWAY;
		}
		else {
			log("frontwalldetected = false.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine rotateToMoveAway() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, ROTATE_ANGLE);

		if (returnVal == ReturnResult.Completed) {
			log("rotateToMoveAway.ObstacleSkipper", 4);
			
			machineState = ObstacleSkipperState.SCAN_AFTER_ROTATE_TO_MOVE_BACK;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterRotateToMoveAway() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.ObstacleSkipper", 4);
			ROTATE_DIRECTION *= -1;
			ROTATE_ANGLE = ROTATE_DIRECTION * 180;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_AWAY;
		}
		else {
			log("frontwalldetected = false.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.MOVE_AWAY;
		}
		
		return machineState;
	}
	
	public IStateMachine moveAway() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveAway.ObstacleSkipper", 4);
			
			++m_deviationCount;
			machineState = ObstacleSkipperState.SCAN_AFTER_MOVE_AWAY;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterMoveAway() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.ObstacleSkipper", 4);
			ROTATE_DIRECTION *= -1;
			ROTATE_ANGLE = ROTATE_DIRECTION * 180;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_AWAY;
		}
		else {
			log("frontwalldetected = false.ObstacleSkipper", 4);
			ROTATE_DIRECTION *= -1;
			ROTATE_ANGLE = ROTATE_DIRECTION * 90;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_FORWARD;
		}
		
		return machineState;
	}
	
	public IStateMachine rotateToMoveForward() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, ROTATE_ANGLE);

		if (returnVal == ReturnResult.Completed) {
			log("rotateToMoveForward.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.SCAN_AFTER_ROTATE_TO_MOVE_FORWARD;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterRotateToMoveForward() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.ObstacleSkipper", 4);
			ROTATE_DIRECTION *= -1;
			ROTATE_ANGLE = ROTATE_DIRECTION * 90;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_AWAY;
		}
		else {
			log("frontwalldetected = false.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.MOVE_FORWARD;
		}
		
		return machineState;
	}
	
	public IStateMachine moveForward() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveForward.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.SCAN_AFTER_MOVE_FORWARD;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterMoveForward() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.ObstacleSkipper", 4);
			ROTATE_DIRECTION *= -1;
			ROTATE_ANGLE = ROTATE_DIRECTION * 90;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_AWAY;
		}
		else if (m_deviationCount <= 0) {
			log("DONE.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.DONE;
		}
		else {
			log("frontwalldetected = false.ObstacleSkipper", 4);
			ROTATE_ANGLE = ROTATE_DIRECTION * 90;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_BACK;
		}
		
		return machineState;
	}
	
	public IStateMachine rotateToMoveBack() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnDegree, ROTATE_ANGLE);

		if (returnVal == ReturnResult.Completed) {
			log("rotateToMoveBack.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.SCAN_AFTER_ROTATE_TO_MOVE_BACK;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterRotateToMoveBack() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.frontwalldetected) {
			log("frontwalldetected = true.ObstacleSkipper", 4);
			ROTATE_DIRECTION *= -1;
			ROTATE_ANGLE = ROTATE_DIRECTION * 90;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_FORWARD;
		}
		else {
			log("frontwalldetected = false.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.MOVE_BACK;
		}
		
		return machineState;
	}
	
	public IStateMachine moveBack() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveBack.ObstacleSkipper", 4);
			--m_deviationCount;
			machineState = ObstacleSkipperState.SCAN_AFTER_MOVE_BACK;
		}
		
		return machineState;
	}
	
	public IStateMachine scanAfterMoveBack() {

		m_robotPosition = m_positionDetector.getPosition();

		if (m_robotPosition.frontwalldetected || m_deviationCount <= 0) {
			log("frontwalldetected = true.ObstacleSkipper", 4);
			ROTATE_DIRECTION *= -1;
			ROTATE_ANGLE = ROTATE_DIRECTION * 90;
			machineState = ObstacleSkipperState.ROTATE_TO_MOVE_FORWARD;
		}
		else {
			log("frontwalldetected = false.ObstacleSkipper", 4);
			machineState = ObstacleSkipperState.MOVE_BACK;
		}

		return machineState;
	}
}